/*
 * platform.cpp
 *
 *  Created on: Dec 4, 2012
 *      Author: michiel
 */

#include <XMLConfiguration.h>

#include "navigation/ps3_control.h"

void Platform::init()
{
  ROS_INFO("Initializing base");

  double Kp, Ki, Kd;
  double i_limit;

  // Read parameters
  std::string motor_port_name, motor_config_name;

  ROS_ASSERT(nh_.getParam("motor_port", motor_port_name));
  ROS_ASSERT(nh_.getParam("motor_config", motor_config_name));
  ROS_ASSERT(nh_.getParam("wheel_diameter", wheel_diameter_));
  ROS_ASSERT(nh_.getParam("wheel_base", wheel_base_));

  // Subscript to command topic
  vel_sub_ = nh_.subscribe(Topic::NAV_VELOCITY, 10, &Platform::velocityCallback, this);

  // Load motor configuration
  CXMLConfiguration motor_config_xml;
  ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

  CDxlConfig motor_config_left;
  motor_config_left.readConfig(motor_config_xml.root().section("left"));
  left_motor_ = new C3mxlROS(motor_port_name.c_str());
  left_motor_->setConfig(&motor_config_left);

  // Initialize left motor
  ros::Rate init_rate(1);
  while (ros::ok() && left_motor_->init() != DXL_SUCCESS)
  {
    ROS_WARN_ONCE("Couldn't initialize left wheel motor, will continue trying every second");
    init_rate.sleep();
  }

  CDxlConfig motor_config_right;
  motor_config_right.readConfig(motor_config_xml.root().section("right"));
  right_motor_ = new C3mxlROS(motor_port_name.c_str());
  right_motor_->setConfig(&motor_config_right);

  // Initialize right motor
  while (ros::ok() && right_motor_->init() != DXL_SUCCESS)
  {
    ROS_WARN_ONCE("Couldn't initialize right wheel motor, will continue trying every second");
    init_rate.sleep();
  }
}

void Platform::spin()
{
  ROS_INFO("Spinning");
  ros::spin();
}

void Platform::velocityCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
  // Base is nonholonomic, warn if sent a command we can't execute
  if (msg->linear.y || msg->linear.z || msg->angular.x || msg->angular.y)
  {
	ROS_WARN("I'm afraid I can't do that, Dave.");
	return;
  }

  // Calculate wheel velocities
  double vel_linear  = msg->linear.x/(wheel_diameter_/2);
  double vel_angular = msg->angular.z * (wheel_base_/wheel_diameter_);

  double vel_left    = vel_linear - vel_angular;
  double vel_right   = vel_linear + vel_angular;

  // Actuate
  left_motor_->setSpeed(vel_left);
  right_motor_->setSpeed(vel_right);

  ROS_DEBUG_STREAM("Base velocity set to [" << vel_left << ", " << vel_right << "]");
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "platform_node");

	Platform platform;
	platform.spin();
}

